state_block.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
/*
 * Macro for defining StateBlocks creators.
 *
 * Place a call to this macro inside your class declaration
 * (in the state_block_class.h file), preferably just after the constructors.
 *
 * In order to use this macro, the derived state_block class, StateBlockClass,
 * must have the constructor callable as:
 *
 *   StateBlockClass(const Eigen::VectorXd& _state, bool _fixed)
 *
 * And the static method:
 *
 *   static Eigen::VectorXd Identity()
 */
#define WOLF_STATE_BLOCK_CREATE(StateBlockClass)                                                                      \
    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed)                                           \
    {                                                                                                                 \
        return std::make_shared<StateBlockClass>(_state, _fixed);                                                     \
    }                                                                                                                 \
    static StateBlockPtr createIdentity(bool _fixed)                                                                  \
    {                                                                                                                 \
        return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), _fixed);                                \
    }
 
// Fwd references
namespace wolf
{
class NodeBase;
class LocalParametrizationBase;
}  // namespace wolf
 
// Wolf includes
#include "core/common/wolf.h"
#include "core/composite/vector_composite.h"
 
// std includes
#include <iostream>
#include <mutex>
 
namespace wolf
{
class StateBlock : public std::enable_shared_from_this<StateBlock>
{
  public:
  protected:
    std::string      type_;   
    std::atomic_bool fixed_;  
 
    std::atomic<SizeEigen> state_size_;  
    Eigen::VectorXd        state_;       
    mutable std::mutex     mut_state_;   
 
    LocalParametrizationBasePtr
        local_param_ptr_;  
 
    std::atomic_bool state_updated_;  
    std::atomic_bool fix_updated_;    
    std::atomic_bool
        local_param_updated_;  
 
    bool transformable_;  
 
  public:
    StateBlock(const std::string&          _type,
               const SizeEigen             _size,
               bool                        _fixed           = false,
               LocalParametrizationBasePtr _local_param_ptr = nullptr,
               bool                        _transformable   = true);
 
    StateBlock(const std::string&          _type,
               const Eigen::VectorXd&      _state,
               bool                        _fixed           = false,
               LocalParametrizationBasePtr _local_param_ptr = nullptr,
               bool                        _transformable   = true);
 
    StateBlock(const StateBlock& o) = delete;
    StateBlock(StateBlock&& o)      = delete;
    StateBlock& operator=(const StateBlock& o) = delete;
 
    virtual ~StateBlock();
 
    std::string getType() const;
 
    Eigen::VectorXd getState() const;
 
    double* getStateData();
 
    void setState(const Eigen::VectorXd& _state, const bool _notify = true);
 
    SizeEigen getSize() const;
 
    SizeEigen getLocalSize() const;
 
    bool isFixed() const;
 
    void fix();
 
    void unfix();
 
    void setFixed(bool _fixed);
 
    bool hasLocalParametrization() const;
 
    LocalParametrizationBasePtr getLocalParametrization() const;
 
    void setLocalParametrization(LocalParametrizationBasePtr _local_param);
 
    void removeLocalParametrization();
 
    bool stateUpdated() const;
 
    bool fixUpdated() const;
 
    bool localParamUpdated() const;
 
    void resetStateUpdated();
 
    void resetFixUpdated();
 
    void resetLocalParamUpdated();
 
    virtual void setIdentity(bool _notify = true);
    void         setZero(bool _notify = true);
 
    virtual Eigen::VectorXd identity() const;
    Eigen::VectorXd         zero() const;
 
    void perturb(double amplitude = 0.1);
 
    bool isTransformable() const
    {
        return transformable_;
    }
 
    void setTransformable(bool _trf = true)
    {
        transformable_ = _trf;
    }
 
    void setNonTransformable()
    {
        transformable_ = false;
    }
 
    virtual void transform(const VectorComposite& _transformation) = 0;
 
    void plus(const Eigen::VectorXd& _dv);
 
    bool isValid(double tolerance = Constants::EPS);
};
 
}  // namespace wolf
 
// IMPLEMENTATION
#include "core/state_block/local_parametrization_base.h"
#include "core/common/node_base.h"
#include "core/problem/problem.h"
 
namespace wolf
{
inline StateBlock::StateBlock(const std::string&          _type,
                              const Eigen::VectorXd&      _state,
                              bool                        _fixed,
                              LocalParametrizationBasePtr _local_param_ptr,
                              bool                        _transformable)
    : type_(_type),
      fixed_(_fixed),
      state_size_(_state.size()),
      state_(_state),
      local_param_ptr_(_local_param_ptr),
      state_updated_(false),
      fix_updated_(false),
      local_param_updated_(false),
      transformable_(_transformable)
{
    //    std::cout << "constructed           +sb" << std::endl;
}
 
inline StateBlock::StateBlock(const std::string&          _type,
                              const SizeEigen             _size,
                              bool                        _fixed,
                              LocalParametrizationBasePtr _local_param_ptr,
                              bool                        _transformable)
    : type_(_type),
      fixed_(_fixed),
      state_size_(_size),
      state_(Eigen::VectorXd::Zero(_size)),
      local_param_ptr_(_local_param_ptr),
      state_updated_(false),
      fix_updated_(false),
      local_param_updated_(false),
      transformable_(_transformable)
{
    //    std::cout << "constructed           +sb" << std::endl;
}
 
inline StateBlock::~StateBlock()
{
    //    std::cout << "destructed            -sb" << std::endl;
}
 
inline std::string StateBlock::getType() const
{
    return type_;
}
 
inline Eigen::VectorXd StateBlock::getState() const
{
    std::lock_guard<std::mutex> lock(mut_state_);
    return state_;
}
 
inline SizeEigen StateBlock::getSize() const
{
    return state_size_.load();
}
 
inline SizeEigen StateBlock::getLocalSize() const
{
    if (local_param_ptr_) return local_param_ptr_->getLocalSize();
    return getSize();
}
 
inline bool StateBlock::isFixed() const
{
    return fixed_.load();
}
 
inline void StateBlock::fix()
{
    setFixed(true);
}
 
inline void StateBlock::unfix()
{
    setFixed(false);
}
 
inline bool StateBlock::hasLocalParametrization() const
{
    return (local_param_ptr_ != nullptr);
}
 
inline LocalParametrizationBasePtr StateBlock::getLocalParametrization() const
{
    return local_param_ptr_;
}
 
inline void StateBlock::removeLocalParametrization()
{
    assert(local_param_ptr_ != nullptr && "state block without local parametrization");
    local_param_ptr_.reset();
    local_param_updated_.store(true);
}
 
inline void StateBlock::setLocalParametrization(LocalParametrizationBasePtr _local_param)
{
    assert(_local_param != nullptr && "setting a null local parametrization");
    local_param_ptr_ = _local_param;
    local_param_updated_.store(true);
}
 
inline bool StateBlock::stateUpdated() const
{
    return state_updated_.load();
}
 
inline bool StateBlock::fixUpdated() const
{
    return fix_updated_.load();
}
 
inline bool StateBlock::localParamUpdated() const
{
    return local_param_updated_.load();
}
 
inline void StateBlock::resetStateUpdated()
{
    state_updated_.store(false);
}
 
inline void StateBlock::resetFixUpdated()
{
    fix_updated_.store(false);
}
 
inline void StateBlock::resetLocalParamUpdated()
{
    local_param_updated_.store(false);
}
 
inline double* StateBlock::getStateData()
{
    return state_.data();
}
 
inline void StateBlock::setIdentity(bool _notify)
{
    setState(Eigen::VectorXd::Zero(state_size_), _notify);
}
 
inline void StateBlock::setZero(bool _notify)
{
    setIdentity(_notify);
}
 
inline Eigen::VectorXd StateBlock::identity() const
{
    return Eigen::VectorXd::Zero(state_size_);
}
 
inline Eigen::VectorXd StateBlock::zero() const
{
    return identity();
}
 
inline bool StateBlock::isValid(double tolerance)
{
    return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
}
 
}  // namespace wolf